%% 滑模控制2用于制导
% 原模型是二体相对运动，这道题简化为目标是静止的，导弹从高处往下投
% 为简化计算过程，预先将目标物相关量置0
% 假设控制系统无滞后am = ac
% ac只用于改变导弹速度方向，不改变导弹速度大小

% TODO: 加速度图与PPT不同

clear all;
clc;

%% 参数初始化
% dX = AX + Bc*ac + Bt*at
% 普通滑膜s=x2+c(x1-lambdaD), ds=-k*s-epsilon*sgn(s)
% 自适应滑膜s=x2+k1*(x1-lambdaD)/tgo, ds=[-k2*s-kappa*sgn(s)]/tgo
gainc = 0.2;
gaink = 0.2;
epsilon = 0.002;
gaink1 = 1;
gaink2 = 1;
kappa = 0.01;

disx = 6000;                %导弹与目标水平距离(m)
disy = 3000;                %导弹与目标高度距离(m)
disr = norm([disx, disy]);  %导弹与目标的直线距离(m)
velM = 260;                 %导弹初始速度(m/s)
thetaM = 0;                 %导弹速度与水平夹角(rad)
etaM = -atan(disy/disx);    %导弹速度与MT连线夹角(rad)
accelM = 0;                 %导弹加速度(m/s^2)
lambda = -atan(disy/disx);  %视线角(rad)
lambdaDesire = -30/180*pi;  %期望视线角(rad)
dLambda = velM*sin(etaM)/disr;
X = [lambda; dLambda];      %初始状态

dt = 0.01;                   %仿真步长(s)
T = 30;                     %仿真周期(s)

%% 画图记录量
time = 0:dt:T;
xyLog1 = zeros(2, T/dt+1);
xyLog2 = zeros(2, T/dt+1);
accelLog = zeros(2, T/dt+1);
lambdaLog = zeros(2, T/dt+1);
sLog = zeros(2, T/dt+1);

%% 普通滑模控制
for i = 1:1:T/dt+1
    % 滑模控制量s和加速度accelC
    dDisr = -velM*cos(etaM);
    matA = [0, 1; 0, -2*dDisr/disr];
    matBc = [0; -cos(etaM)/disr];
    
    matC = [gainc, 1];
    s = matC*X - matC(1)*lambdaDesire;
    accelC = (-matC*matA*X -epsilon*sat(s) -gaink*s) / (matC*matBc);
    
    % 状态方程
    dX = matA*X + matBc*accelC;
    
    % 状态递推
    X = X + dX*dt;
    lambda = X(1);
    dLambda = X(2);
    
    accelM = accelC;
    dThetaM = accelM / velM;
    thetaM = thetaM + dThetaM*dt;
    
    etaM = lambda - thetaM;
    disr = disr -velM*cos(etaM)*dt;
    
    if disr <= 0
        disr = 0;
        % 写入剩下的数
        for j = i:1:T/dt+1
            xyLog1(:, j) = [disr*cos(lambda); disr*sin(lambda)];
            accelLog(1, j) = accelM;
            lambdaLog(1, j) = lambda*180/pi;
            sLog(1, j) = s;
        end
        break;
    end
    
    % 记录
    xyLog1(:, i) = [-disr*cos(lambda); -disr*sin(lambda)];
    accelLog(1, i) = accelM;
    lambdaLog(1, i) = lambda*180/pi;
    sLog(1, i) = s;

end

%% 自适应滑模控制
disr = norm([disx, disy]);  %导弹与目标的直线距离(m)
thetaM = 0;                 %导弹速度与水平夹角(rad)
etaM = -atan(disy/disx);    %导弹速度与MT连线夹角(rad)
lambda = -atan(disy/disx);  %视线角(rad)
dLambda = velM*sin(etaM)/disr;
X = [lambda; dLambda];      %初始状态

for i = 1:1:T/dt+1
    % 滑模控制量s和加速度accelC
    dDisr = -velM*cos(etaM);
    tgo = -disr/dDisr;
    matA = [0, 1; 0, -2*dDisr/disr];
    matBc = [0; -cos(etaM)/disr];
    matC = [gaink1/tgo, 1];
    s = matC*X - matC(1)*lambdaDesire;
    
    accelC = disr*( dLambda*(-2*dDisr/disr + (gaink1+gaink2)/tgo) +...
                gaink1*(gaink2+1)*(lambda-lambdaDesire)/tgo^2 +...
                kappa*sat(s)/tgo );
    accelC = accelC / cos(etaM);
    
    % 状态方程
    dX = matA*X + matBc*accelC;
    
    % 状态递推
    X = X + dX*dt;
    lambda = X(1);
    dLambda = X(2);
    
    accelM = accelC;
    dThetaM = accelM / velM;
    thetaM = thetaM + dThetaM*dt;
    
    etaM = lambda - thetaM;
    %disr = velM*sin(etaM)/dLambda; %不能用这条进行递推
    disr = disr -velM*cos(etaM)*dt;
    if disr<=0
        disr = 0;
        % 写入剩下的数
        for j = i:1:T/dt+1
            xyLog2(:, j) = [disr*cos(lambda); disr*sin(lambda)];
            accelLog(2, j) = accelM;
            lambdaLog(2, j) = lambda*180/pi;
            sLog(2, j) = s;
        end
        break;
    end
    
    % 记录
    xyLog2(:, i) = [-disr*cos(lambda); -disr*sin(lambda)];
    accelLog(2, i) = accelM;
    lambdaLog(2, i) = lambda*180/pi;
    sLog(2, i) = s;

end

%% 画图
figure;
plot(xyLog1(1,:), xyLog1(2,:),'linewidth', 1.5);
hold on;
plot(xyLog2(1,:), xyLog2(2,:), 'linewidth', 1.5);
grid on;
title('导弹飞行轨迹图');
xlabel('x(m)');
ylabel('h(m)');
legend('普通', '自适应');

figure;
plot(time, accelLog, 'linewidth', 1.5);
hold on;
grid on;
title('导弹加速度变化图');
xlabel('time(s)');
ylabel('accelM(m/s^2)');
legend('普通', '自适应');

figure;
plot(time, lambdaLog, 'linewidth', 1.5);
hold on;
grid on;
title('视线角变化图');
xlabel('time(s)');
ylabel('lambda(°)');
legend('普通', '自适应');

figure;
plot(time, sLog, 'linewidth', 1.5);
hold on;
grid on;
title('s函数变化图');
xlabel('time(s)');
ylabel('s');
legend('普通', '自适应');

%% 小函数
function y = sat(x)
    if x >= 0.1
        y = 1;
    elseif x <= -0.1
        y = -1;
    else
        y = x/0.1;
    end
end
